#include "CANqueue.h"
#include <string.h>

CanQueue_T CAN1_queue = {0};

static void _can_push(CanQueue_T* cq, CanRxMsg* m)
{
    cq->buf[cq->tail] = *m;
    cq->tail++;
    if(cq->tail==CANQUEUE_BUFSIZE)
        cq->tail = 0;
    cq->size++;
    cq->is_empty = 0;
    if(cq->size>=CANQUEUE_BUFSIZE)
    {
        cq->size = CANQUEUE_BUFSIZE;
        cq->head = cq->tail;
    }
    cq->front_ptr = &cq->buf[cq->head];
}

static CanRxMsg _can_pop(CanQueue_T* cq)
{
    uint8_t ret = cq->head;
    if(cq->size>0)
    {
        cq->head++;
        if(cq->head==CANQUEUE_BUFSIZE)
            cq->head=0;
        cq->size--;
        if(cq->size==0)cq->is_empty = 1;
        return cq->buf[ret];
    }
    else
        return cq->buf[0];
}

void can1_push(CanRxMsg* m)
{
    _can_push(&CAN1_queue, m);
}

CanRxMsg can1_pop()
{
    return _can_pop(&CAN1_queue);
}

void canqueue_init(void)
{
    memset(&CAN1_queue, 0, sizeof(CAN1_queue));
    CAN1_queue.is_empty = 1;
    CAN1_queue.push = can1_push;
    CAN1_queue.pop = can1_pop;
}
